#include <cv.h>
#include <highgui.h>

#include "ros/ros.h"
#include <std_msgs/Int32.h>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "robo/CameraParams.h"
#include <string.h>
#include <signal.h>

using namespace robo;
namespace enc = sensor_msgs::image_encodings;

void cam0_cb(const sensor_msgs::ImageConstPtr& msg)
{
	cv_bridge::CvImagePtr cv_ptr;
	try
	{
		cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
		cv::Mat mat = cv_ptr->image;
		IplImage img = mat;
		cvNamedWindow("Cam0_recive", CV_WINDOW_AUTOSIZE );
		cvShowImage("Cam0_recive", &img);
		cvWaitKey(30);
	}
	catch (cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
		return;
	}
}

void cam1_cb(const sensor_msgs::ImageConstPtr& msg)
{
	cv_bridge::CvImagePtr cv_ptr;
	try
	{
		cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
		cv::Mat mat = cv_ptr->image;
		IplImage img = mat;
		cvNamedWindow("Cam1_recive", CV_WINDOW_AUTOSIZE );
		cvShowImage("Cam1_recive", &img);
		cvWaitKey(30);
	}
	catch (cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
		return;
	}
}

int main(int argc, char **argv)
{
	//signal(SIGINT, sigtrap);
	
	printf("Starting CameraNodeTest\n");
	ros::init(argc, argv, "CameraNodeTest");
	ros::NodeHandle n;
	image_transport::ImageTransport it(n);
	image_transport::Subscriber img0_sub = it.subscribe("/camera0_img", 1, cam1_cb);
	image_transport::Subscriber img1_sub = it.subscribe("/camera1_img", 1, cam1_cb);
	//ros::Subscriber sub 	= n.subscribe("/camera_params"	, 100, change_camera_params);
	//image_transport::ImageTransport it_(n);
	//image_transport::Publisher img0_pub = it_.advertise("/camera0_img", 1);
	ros::spin();
}
